// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org and
// the qglviewer library : http://www.libqglviewer.com


#ifndef _OpenMeca_Geom_QUATERNION_hpp_
#define _OpenMeca_Geom_QUATERNION_hpp_

#include <cmath>
#include <iostream>
#include <boost/bind.hpp>

#include "OpenMeca/Geom/SpaceDim.hpp"
#include "OpenMeca/Geom/Vector.hpp"
#include "OpenMeca/Geom/Coordinate.hpp"
#include "OpenMeca/Geom/Frame.hpp"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

namespace OpenMeca
{
  namespace Geom
  {
    
    template<SpaceDim N> class Matrix;

    template<SpaceDim N>
    class Quaternion
    {
      template<SpaceDim M> friend std::ostream & operator<< (std::ostream& o, const Quaternion<M>& q);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const Vector<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Vector<M> &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const Quaternion<M> &, const double &);
      template<SpaceDim M> friend Quaternion<M> operator*  (const double &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator+  (const Quaternion<M> &, const Quaternion<M> &);
      template<SpaceDim M> friend Quaternion<M> operator-  (const Quaternion<M> &, const Quaternion<M> &);

    public:
      static std::string GetStrKey(){return std::string("Quaternion" + SpaceDimUtil<N>::GetStrKey());}

    public:
      //CONSTRUCTORS & DESTRUCTORS
      Quaternion(boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      Quaternion(double , double , double , double , boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      Quaternion(double , double , double , double , boost::function<const Frame<N>& ()>, const Frame<N>&);
      Quaternion(const Vector<_3D>&, double, boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal);
      Quaternion(const Quaternion<N> &);
      Quaternion(const Quaternion<N> &, boost::function<const Frame<N>& ()>);
      Quaternion(const Vector<N> &, const Vector<N> &);

     
     //ACCESSORS
      void SetVecFromTo(const Vector<N>& from, const Vector<N>& to);
      void SetAxisAngle(const Vector<N>& axis, double angle);
      void SetValue(double q0, double q1, double q2, double q3);
      void SetFrameTo(const Frame<N>*);

      const Frame<N> & GetFrameFrom() const;
      const Frame<N> & GetFrameTo() const;
      const Frame<N>* GetFrameToPtr() const{return frameTo_;}; 

      Vector<N> GetAxis() const;
      double GetAngle() const;
      void GetAxisAngle(Vector<N>& axis, double& angle) const;
      Vector<N> ToVector(boost::function<const Frame<N>& ()> = &Frame<N>::GetGlobal) const;

      //OPERATORS
      Quaternion& operator=(const Quaternion& Q);
      const double& operator[](int i) const;
      double& operator[](int i);

      const double & GetReal() const;
      double& GetReal();
      const Coordinate<N> & GetCoordinate() const{return q_;};
      Coordinate<N> & GetCoordinate(){return q_;};

      Quaternion& operator*=(const Quaternion &Q);
      Quaternion& operator*=(const Vector<N> &v);
      Quaternion& operator*=(const double &d);
      Quaternion& operator+=(const Quaternion &Q);
      Quaternion& operator-=(const Quaternion &Q);


      //UTILS
      void Rotate(const Vector<N> & v_in, Vector<N> & v_out) const;
      void InverseRotate(const Vector<N> & v_in, Vector<N> & v_out) const;

      Vector<N> Rotate(const Vector<N> & v_in) const;
      Vector<N> InverseRotate(const Vector<N> & v_in) const;

      void Clear();
      Quaternion GetConjugate() const;
      void Invert();
      void Negate();
      double GetNorm() const;
      double GetSquaredNorm() const;
      double Normalize();
      Quaternion GetNormalized() const;
      SpaceDim GetDimension() const ;
      Matrix<N> ToRotationMatrix() const;
      std::ostream & Write (std::ostream & out) const;

            boost::function<const Frame<N>& ()>& GetFrameFunctionAccess() {return q_.GetFrameFunctionAccess();}
      const boost::function<const Frame<N>& ()>& GetFrameFunctionAccess() const {return q_.GetFrameFunctionAccess();}


    private:
      void EqualComponent(const Quaternion<_3D> &);
      // - BOOST SERIALIZATION - //
      friend class boost::serialization::access;
      template<class Archive> void serialize(Archive & ar, const unsigned int );
      
   
    private:
      double real_;
      Coordinate<N> q_;
      const Frame<N> *frameTo_;
    };


    // - BOOST SERIALIZATION - //  
    template<SpaceDim N>
    template<class Archive>
    inline void 
    Quaternion<N>::serialize(Archive & ar, const unsigned int ) 
    {
      ar  &BOOST_SERIALIZATION_NVP(q_);
      ar  &BOOST_SERIALIZATION_NVP(real_);
    }

    //EXTERN OPERATOR
    template<SpaceDim N>
    std::ostream & operator<< (std::ostream& o, const Quaternion<N>& q);

    template<SpaceDim N>
    Quaternion<N> operator*  (const Quaternion<N> &, const Vector<N> &);

    template<SpaceDim N>
    Quaternion<N> operator*  (const Vector<N> &, const Quaternion<N> &);

    template<SpaceDim N>
    Quaternion<N> operator*  (const Quaternion<N> &, const Quaternion<N> &);

    template<SpaceDim N>
    Quaternion<N> operator*  (const Quaternion<N> &, const double &);

    template<SpaceDim N>
    Quaternion<N> operator*  (const double &, const Quaternion<N> &);

    template<SpaceDim N>
    Quaternion<N> operator+  (const Quaternion<N> &, const Quaternion<N> &);

    template<SpaceDim N>
    Quaternion<N> operator-  (const Quaternion<N> &, const Quaternion<N> &);


    template<SpaceDim N>
    inline SpaceDim
    Quaternion<N>::GetDimension() const
    {
      return q_.dimension;
    }

    template<SpaceDim N>
    inline const Frame<N> & 
    Quaternion<N>::GetFrameFrom() const
    {
      return q_.GetFrame();
    }

    template<SpaceDim N>
    inline const Frame<N> & 
    Quaternion<N>::GetFrameTo() const
    {
      assert(frameTo_!=0);
      return *frameTo_;
    }

    template<SpaceDim N>
    inline const double &
    Quaternion<N>::GetReal() const
    {
      return real_;
    }

    template<SpaceDim N>
    inline double &
    Quaternion<N>::GetReal()
    {
      return real_;
    }

    template<SpaceDim N>
    inline const double &
    Quaternion<N>::operator[](int i) const
    {
      assert(i>=0 && i <N);
      return q_.c_[i];    
    }

    template<SpaceDim N>
    inline double &
    Quaternion<N>::operator[](int i) 
    {
      assert(i>=0 && i <N);
     return q_.c_[i];
    }

    template<SpaceDim N>
    inline void
    Quaternion<N>::SetFrameTo(const Frame<N>* frameTo)
    {
      frameTo_ = frameTo;
    }

    template<SpaceDim N> inline
    void 
    Quaternion<N>::SetVecFromTo(const Vector<N>& from, const Vector<N>& to)
    {
      assert(GetFrameFrom()==from.GetFrame());
      assert(GetFrameFrom()==to.GetFrame());
      const float epsilon = 1E-10f;
      
      const float fromSqNorm = from.GetSquaredNorm();
      const float toSqNorm   = to.GetSquaredNorm();
      // Identity Quaternion when one vector is null
      if ((fromSqNorm < epsilon) || (toSqNorm < epsilon))
	{
	  q_[0]=q_[1]=q_[2]=0.;
	  real_=1.;
	}
      else
	{
	  Vector<N> axis = from^to;
	  const float axisSqNorm = axis.GetSquaredNorm();
	  
	  // Aligned vectors, pick any axis, not aligned with from or to
	  if (axisSqNorm < epsilon)
	    {
	      axis = from.GetOrthogonalVector();
	    }
	  
	  double angle = asin(sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
	  
	  if (from*to < 0.0)
	    {
	      angle = M_PI-angle;
	    }
	  
	  SetAxisAngle(axis, angle);
	}

    }
     
    template<SpaceDim N> inline
    Quaternion<N>::Quaternion(const Vector<N> &from, const Vector<N> &to)
      :q_(from.GetFrame()), frameTo_(&Frame<N>::Null)
    {
      SetVecFromTo(from,to);
    }
    

    //***********************************************************
    //3D SPECIALISATION

    template<>
    inline void
    Quaternion<_3D>::SetAxisAngle(const Vector<_3D>& axis, double angle)
    {
      assert(GetFrameFrom()==axis.GetFrame());
      const double norm = axis.GetNorm();
      // if (norm < epsilon)
      //   {
      //     q_.c_[0] = 0.0; 
      //     q_.c_[1] = 0.0;
      //     q_.c_[2] = 0.0;
      //     real_ = 1.0;
      //    }
      // else
      // {
      const double sin_half_angle = sin(angle / 2.0);
      q_.c_[0] = sin_half_angle*axis.coord_.c_[0]/norm;
      q_.c_[1] = sin_half_angle*axis.coord_.c_[1]/norm;
      q_.c_[2] = sin_half_angle*axis.coord_.c_[2]/norm;
      real_ = cos(angle / 2.0);
      //}
    }


    //
    // Constructors
    //
    template<> inline
    Quaternion<_3D>::Quaternion(boost::function<const Frame<_3D>& ()>f)
      : real_(1.), q_(0.,0.,0.,f), frameTo_(&Frame<_3D>::Null)
    {
     
    }

    template<> inline
    Quaternion<_3D>::Quaternion(const Vector<_3D>& axis, double angle, boost::function<const Frame<_3D>& ()> f)
      : real_(1.), q_(0.,0.,0.,f), frameTo_(&Frame<_3D>::Null)
    {
      SetAxisAngle(axis, angle);
    }

    template<> inline
    Quaternion<_3D>::Quaternion(double q0, double q1, double q2, double real, boost::function<const Frame<_3D>& ()> f)
      : real_(real), q_(q0,q1,q2,f), frameTo_(&Frame<_3D>::Null)
    {
    }

    template<> inline
    Quaternion<_3D>::Quaternion(double q0, double q1, double q2, double real, boost::function<const Frame<_3D>& ()> f, const Frame<_3D>& to)
      :real_(real), q_(q0,q1,q2,f), frameTo_(&to)
    {
    }

    
    template<> inline
    Quaternion<_3D>::Quaternion(const Quaternion<_3D>& Q)
      : real_(Q.real_), q_(Q.q_), frameTo_(&Frame<_3D>::Null)
    {
    }


    template<> inline 
    std::ostream&
    operator<<(std::ostream& o, const Quaternion<_3D>& q)
    {
      return o << q.q_.c_[0] << '\t' << q.q_.c_[1] << '\t' << q.q_.c_[2] << '\t' << q.GetReal();
    }

    template<> inline 
    Quaternion<_3D>
    operator*(const Quaternion<_3D>& q1, const Quaternion<_3D>& q2)
    {
       return Quaternion<_3D> (q1.real_*q2.q_.c_[0]  + q1.q_.c_[0]*q2.real_  + q1.q_.c_[1]*q2.q_.c_[2] - q1.q_.c_[2]*q2.q_.c_[1],
			 q1.real_*q2.q_.c_[1]        + q1.q_.c_[1]*q2.real_  + q1.q_.c_[2]*q2.q_.c_[0] - q1.q_.c_[0]*q2.q_.c_[2],
			 q1.real_*q2.q_.c_[2]        + q1.q_.c_[2]*q2.real_  + q1.q_.c_[0]*q2.q_.c_[1] - q1.q_.c_[1]*q2.q_.c_[0],
			 q1.real_*q2.real_ - q1.q_.c_[0]* q2.q_.c_[0]        - q1.q_.c_[1]*q2.q_.c_[1] - q1.q_.c_[2]*q2.q_.c_[2],
			 q1.GetFrameFunctionAccess(), q2.GetFrameTo() );
    }

    template<> inline 
    Quaternion<_3D>
    operator*(const Quaternion<_3D>& q, const Vector<_3D>& v)
    {
      return Quaternion<_3D>(q.real_*v.coord_.c_[0] + q.q_.c_[1]*v.coord_.c_[2] - q.q_.c_[2]*v.coord_.c_[1],
                             q.real_*v.coord_.c_[1] + q.q_.c_[2]*v.coord_.c_[0] - q.q_.c_[0]*v.coord_.c_[2],
                             q.real_*v.coord_.c_[2] + q.q_.c_[0]*v.coord_.c_[1] - q.q_.c_[1]*v.coord_.c_[0],
                             - v.coord_.c_[0]*q.q_.c_[0] - q.q_.c_[1]*v.coord_.c_[1] - q.q_.c_[2]*v.coord_.c_[2] );
    }

    template<> inline 
    Quaternion<_3D>
    operator*  (const Vector<_3D>& v, const Quaternion<_3D>& q)
    {
      return Quaternion<_3D>(q.real_*v.coord_.c_[0] + v.coord_.c_[1]*q.q_.c_[2] - v.coord_.c_[2]*q.q_.c_[1],
                             q.real_*v.coord_.c_[1] + v.coord_.c_[2]*q.q_.c_[0] - v.coord_.c_[0]*q.q_.c_[2],
                             q.real_*v.coord_.c_[2] + v.coord_.c_[0]*q.q_.c_[1] - v.coord_.c_[1]*q.q_.c_[0],
                             - q.q_.c_[0]*v.coord_.c_[0] - v.coord_.c_[1]*q.q_.c_[1] - v.coord_.c_[2]*q.q_.c_[2] );
    }

    template<> inline 
    Quaternion<_3D>
    operator*  (const Quaternion<_3D>& q, const double& d)
    {
      return Quaternion<_3D>(  q.q_.c_[0]*d , q.q_.c_[1]*d ,  q.q_.c_[2]*d  , q.real_*d );
    }


    template<> inline 
    Quaternion<_3D>
    operator*  (const double& d, const Quaternion<_3D>& q)
    {
      return Quaternion<_3D>(  q.q_.c_[0]*d , q.q_.c_[1]*d ,  q.q_.c_[2]*d  , q.real_*d );
    }

    template<> inline 
    Quaternion<_3D>
    operator+  (const Quaternion<_3D>& q1, const Quaternion<_3D>& q2)
    {
      return Quaternion<_3D>(  q1.q_.c_[0]+q2.q_.c_[0] , q1.q_.c_[1]+q2.q_.c_[1] ,  q1.q_.c_[2]+q2.q_.c_[2]  , q1.real_+q2.real_  );
    }

    template<> inline 
    Quaternion<_3D>
    operator-  (const Quaternion<_3D>& q1, const Quaternion<_3D>& q2)
    {
      return Quaternion<_3D>(  q1.q_.c_[0]-q2.q_.c_[0] , q1.q_.c_[1]-q2.q_.c_[1] ,  q1.q_.c_[2]-q2.q_.c_[2]  , q1.real_-q2.real_  );
    }
    
    template<> inline 
    void
    Quaternion<_3D>::SetValue(double q0, double q1, double q2, double q3)
    {
      q_.c_[0]=q0;
      q_.c_[1]=q1;
      q_.c_[2]=q2;
      real_=q3;
    }

    template<> inline 
    Quaternion<_3D>& Quaternion<_3D>::operator=(const Quaternion<_3D>& Q)
    {
      assert(&GetFrameFrom() == &Q.GetFrameFrom());
      //assert(frameTo_ == &Q.GetFrameTo());
      q_.c_[0] = Q.q_.c_[0];
      q_.c_[1] = Q.q_.c_[1];
      q_.c_[2] = Q.q_.c_[2];
      real_ = Q.real_;
      return (*this);
    }

    template<> inline 
    Quaternion<_3D>&
    Quaternion<_3D>::operator*=(const Vector<_3D> &v)
    {
      const double q0(q_.c_[0]);
      const double q1(q_.c_[1]);
      const double q2(q_.c_[2]);
      const double real(real_);
      q_.c_[0] =   real*v.coord_.c_[0] + q1*v.coord_.c_[2] - q2*v.coord_.c_[1];
      q_.c_[1] =   real*v.coord_.c_[1] + q2*v.coord_.c_[0] - q0*v.coord_.c_[2];
      q_.c_[2] =   real*v.coord_.c_[2] + q0*v.coord_.c_[1] - q1*v.coord_.c_[0];
      real_ = - v.coord_.c_[0]*q0   - q1*v.coord_.c_[1] - q2*v.coord_.c_[2];
      return *this;
    }
    
    template<> inline 
    Quaternion<_3D>&
    Quaternion<_3D>::operator*=(const double &d)
    {
      q_.c_[0] *= d;
      q_.c_[1] *= d;
      q_.c_[2] *= d;
      real_ *= d;
      return *this;
    }

    template<> inline 
    Quaternion<_3D>&
    Quaternion<_3D>::operator*=(const Quaternion<_3D> &q)
    {
      const double q0(q_.c_[0]);
      const double q1(q_.c_[1]);
      const double q2(q_.c_[2]);
      const double real(real_);
      q_.c_[0] = real*q.q_.c_[0]        + q0*q.GetReal() + q1*q.q_.c_[2] - q2*q.q_.c_[1];
      q_.c_[1] = real*q.q_.c_[1]        + q1*q.GetReal() + q2*q.q_.c_[0] - q0*q.q_.c_[2];
      q_.c_[2] = real*q.q_.c_[2]        + q2*q.GetReal() + q0*q.q_.c_[1] - q1*q.q_.c_[0];
      real_ = real*q.GetReal() - q0*q.q_.c_[0]        - q1*q.q_.c_[1] - q2*q.q_.c_[2];
      return *this;
    }

    template<> inline 
    Quaternion<_3D>&
    Quaternion<_3D>::operator+=(const Quaternion<_3D> &Q)
    {
      q_.c_[0] += Q.q_.c_[0];
      q_.c_[1] += Q.q_.c_[1];
      q_.c_[2] += Q.q_.c_[2];
      real_ += Q.real_;
      return *this;
    }

    template<> inline 
    Quaternion<_3D>&
    Quaternion<_3D>::operator-=(const Quaternion<_3D> &Q)
    {
      q_.c_[0] -= Q.q_.c_[0];
      q_.c_[1] -= Q.q_.c_[1];
      q_.c_[2] -= Q.q_.c_[2];
      real_ -= Q.real_;
      return *this;
    }

    template<> inline 
    Quaternion<_3D>
    Quaternion<_3D>::GetConjugate() const
    {
      //Need to invert frameFrom & frameTo
      return Quaternion<_3D> (-q_.c_[0], -q_.c_[1], -q_.c_[2], real_, GetFrameFunctionAccess(), GetFrameFrom());
    }

    template<> inline 
    double
    Quaternion<_3D>::GetSquaredNorm() const
    {
      return (q_.c_[0]*q_.c_[0] + q_.c_[1]*q_.c_[1] + q_.c_[2]*q_.c_[2] + real_*real_);
    }

    template<> inline 
    double
    Quaternion<_3D>::GetNorm() const
    {
      return sqrt(q_.c_[0]*q_.c_[0] + q_.c_[1]*q_.c_[1] + q_.c_[2]*q_.c_[2] + real_*real_);
    }

    template<> inline 
    double
    Quaternion<_3D>::Normalize()
    {
      const double norm = sqrt(q_.c_[0]*q_.c_[0] + q_.c_[1]*q_.c_[1] + q_.c_[2]*q_.c_[2] + real_*real_);
      q_.c_[0] /= norm;
      q_.c_[1] /= norm;
      q_.c_[2] /= norm;
      real_ /= norm;
      return norm;
    }

    template<> inline 
    Quaternion<_3D>
    Quaternion<_3D>::GetNormalized() const
    {
      double Q[4];
      const double norm = sqrt(q_.c_[0]*q_.c_[0] + q_.c_[1]*q_.c_[1] + q_.c_[2]*q_.c_[2] + real_*real_);
      Q[0] = q_.c_[0] / norm;
      Q[1] = q_.c_[1] / norm;
      Q[2] = q_.c_[2] / norm;
      Q[3] = real_ / norm;
      return Quaternion<_3D>(Q[0], Q[1], Q[2], Q[3]);
    }

    template<> inline 
    void
    Quaternion<_3D>::Rotate(const Vector<_3D>& v_in, Vector<_3D>& v_out) const
    {
      const double q00 = 2.0 * q_.c_[0] * q_.c_[0];
      const double q11 = 2.0 * q_.c_[1] * q_.c_[1];
      const double q22 = 2.0 * q_.c_[2] * q_.c_[2];
      const double q01 = 2.0 * q_.c_[0] * q_.c_[1];
      const double q02 = 2.0 * q_.c_[0] * q_.c_[2];
      const double q03 = 2.0 * q_.c_[0] * real_;
      const double q12 = 2.0 * q_.c_[1] * q_.c_[2];
      const double q13 = 2.0 * q_.c_[1] * real_;
      const double q23 = 2.0 * q_.c_[2] * real_;

      v_out.coord_.c_[0] = (1.0 - q11 - q22) * (v_in[0]) + (q01 - q23) * v_in[1] + (q02 + q13) * v_in[2];
      v_out.coord_.c_[1] = (q01 + q23) * v_in[0] + (1.0 - q22 - q00) * v_in[1] + (q12 - q03) * v_in[2];
      v_out.coord_.c_[2] = (q02 - q13) * v_in[0] + (q12 + q03) * v_in[1] + (1.0 - q11 - q00) * v_in[2];
    }

    template<> inline 
    void
    Quaternion<_3D>::InverseRotate(const Vector<_3D> & v_in, Vector<_3D> & v_out) const
    {
      const double q00 = 2.0 * q_.c_[0] * q_.c_[0];
      const double q11 = 2.0 * q_.c_[1] * q_.c_[1];
      const double q22 = 2.0 * q_.c_[2] * q_.c_[2];
      const double q01 = 2.0 * q_.c_[0] * q_.c_[1];
      const double q02 = 2.0 * q_.c_[0] * q_.c_[2];
      const double q03 = - 2.0 * q_.c_[0] * real_;
      const double q12 = 2.0 * q_.c_[1] * q_.c_[2];
      const double q13 = - 2.0 * q_.c_[1] * real_;
      const double q23 = - 2.0 * q_.c_[2] * real_;
      
      v_out.coord_.c_[0] = (1.0 - q11 - q22)*v_in[0] + (q01 - q23)*v_in[1] + (q02 + q13)*v_in[2];
      v_out.coord_.c_[1] = (q01 + q23)*v_in[0] + (1.0 - q22 - q00)*v_in[1] + (q12 - q03)*v_in[2];
      v_out.coord_.c_[2] = (q02 - q13)*v_in[0] + (q12 + q03)*v_in[1] + (1.0 - q11 - q00)*v_in[2];
    }

    template<> inline 
    Vector<_3D>
    Quaternion<_3D>::Rotate(const Vector<_3D> & v_in) const
    {
      const double q00 = 2.0 * q_.c_[0] * q_.c_[0];
      const double q11 = 2.0 * q_.c_[1] * q_.c_[1];
      const double q22 = 2.0 * q_.c_[2] * q_.c_[2];
      const double q01 = 2.0 * q_.c_[0] * q_.c_[1];
      const double q02 = 2.0 * q_.c_[0] * q_.c_[2];
      const double q03 = 2.0 * q_.c_[0] * real_;
      const double q12 = 2.0 * q_.c_[1] * q_.c_[2];
      const double q13 = 2.0 * q_.c_[1] * real_;
      const double q23 = 2.0 * q_.c_[2] * real_;
      
      return Vector<_3D> ((1.0 - q11 - q22)*v_in[0] + (q01 - q23)*v_in[1] + (q02 + q13)*v_in[2],
			  (q01 + q23)*v_in[0] + (1.0 - q22 - q00)*v_in[1] + (q12 - q03)*v_in[2],
			  (q02 - q13)*v_in[0] + (q12 + q03)*v_in[1] + (1.0 - q11 - q00)*v_in[2],
			  GetFrameFunctionAccess());
      
    }
    
    template<> inline
    Vector<_3D>
    Quaternion<_3D>::InverseRotate(const Vector<_3D> & v_in) const
    {
      const double q00 = 2.0 * q_.c_[0] * q_.c_[0];
      const double q11 = 2.0 * q_.c_[1] * q_.c_[1];
      const double q22 = 2.0 * q_.c_[2] * q_.c_[2];
      const double q01 = 2.0 * q_.c_[0] * q_.c_[1];
      const double q02 = 2.0 * q_.c_[0] * q_.c_[2];
      const double q03 = - 2.0 * q_.c_[0] * real_;
      const double q12 = 2.0 * q_.c_[1] * q_.c_[2];
      const double q13 = - 2.0 * q_.c_[1] * real_;
      const double q23 = - 2.0 * q_.c_[2] * real_;
      
      
      boost::function<const Frame<_3D>& ()> f = boost::bind(&Quaternion<_3D>::GetFrameTo, boost::ref(*this));

      return Vector<_3D> ((1.0 - q11 - q22)*v_in[0] + (q01 - q23)*v_in[1] + (q02 + q13)*v_in[2],
			  (q01 + q23)*v_in[0] + (1.0 - q22 - q00)*v_in[1] + (q12 - q03)*v_in[2],
			  (q02 - q13)*v_in[0] + (q12 + q03)*v_in[1] + (1.0 - q11 - q00)*v_in[2],
			  f);
    }

    template<> inline 
    void
    Quaternion<_3D>::Clear()
    {
      q_.c_[0] = 0.;
      q_.c_[1] = 0.;
      q_.c_[2] = 0.;
      real_ = 0.;
    }
    
    
    template<> inline 
    Vector<_3D> 
    Quaternion<_3D>::ToVector(boost::function<const Frame<_3D>& ()> f) const
    {
      return Vector<_3D>(q_.c_[0],q_.c_[1],q_.c_[2], f);
    }

    template<> inline 
    void 
    Quaternion<_3D>::EqualComponent(const Quaternion<_3D> &Q)
    {
      q_.c_[0] = Q.q_.c_[0];
      q_.c_[1] = Q.q_.c_[1];
      q_.c_[2] = Q.q_.c_[2];
      real_ = Q.real_;
    }

    template<> inline 
    std::ostream &
    Quaternion<_3D>::Write(std::ostream & out) const
    {
      return out << "Quaternion<_3D> (" << q_.c_[0] << ", " << q_.c_[1] << ", " << q_.c_[2] << ", " << real_ << ")";
    }
      
    //ENDS OF 3D SPECIALIZATION
    //***********************************

    template<SpaceDim N> inline
    Quaternion<N>::Quaternion(const Quaternion<N>& q, boost::function<const Frame<N>& ()> f)
      : real_(q.real_), q_(q.q_.c_[0],q.q_.c_[1],q.q_.c_[2],f), frameTo_(&q.GetFrameTo())
    {
      
      if (q.GetFrameTo() != Frame<_3D>::Null && q.GetFrameTo()==f() && q.GetFrameFrom()==f())
	{
	  std::cout << "&q.GetFrameTo()==&f && &q.GetFrameFrom()==&f" << std::endl << std::flush;
	  assert(0);
	}
      
      else if (q.GetFrameTo() != Frame<_3D>::Null && q.GetFrameTo() == f())
	{
	  std::cout << "&q.GetFrameTo()!=0 && &q.GetFrameTo()==&f" << std::endl << std::flush;
	  assert(0);
	  //frameTo_ = &q.GetFrameFrom();
	  //this->operator=(q.GetConjugate());
	}

      else if (q.GetFrameFrom() == f())
	{
	  EqualComponent(q);
	}
      else
	{
	  //TODO : Faire une méthode plus propre et surtout plus rapide
	  //No common Frame
	  std::vector< Quaternion<_3D> > quaternions;
	  const Frame<N> *refFrame = &q.GetFrameFrom();
	  quaternions.push_back(q);
	  while(refFrame!=&Frame<N>::Global)
	    {
	      
	      quaternions.push_back(refFrame->GetQuaternion());
	      refFrame = &refFrame->GetReferenceFrame();
	    }
	  
	  //
	  // Build Parent Frame tree  -> to be able to scan inverse tree later
	  //
	  std::vector<const Frame<N>*> vectorFrame;
	  refFrame = &f();
	  while(refFrame!=&Frame<N>::Global)
	    {
	      vectorFrame.push_back(refFrame);
	      refFrame = &refFrame->GetReferenceFrame();
	    }
	  
	  for (int i = vectorFrame.size()-1; i>=0; --i)
	    {
	      quaternions.push_back(vectorFrame[i]->GetQuaternion().GetConjugate());
	    }

	  EqualComponent(quaternions[quaternions.size()-1]);
	  for (int i = quaternions.size()-2; i>=0; --i)
	    {
	      (*this)*=quaternions[i];
	    }
	}
    }
    
  }
}

#endif
